#ifndef IO_MODULE__TIME_STAMP_HPP_
#define IO_MODULE__TIME_STAMP_HPP_

#include "rclcpp/rclcpp.hpp"
#include "io_macro.h"
#include <mutex>
#include <thread>
#include <memory>
#include <chrono>

namespace io_module {

class CTimeStamp {
public:
    CTimeStamp() = default;
    explicit CTimeStamp(const std::shared_ptr<rclcpp::Node>& node)
     : node_(node), time_point_(node->now()) {}

    rclcpp::Time get() const { return time_point_; }
    void update() { time_point_ = node_->now(); }
    
    bool is_new(uint16_t ms = 500) const {
        return (node_->now() - time_point_).nanoseconds() < ms * 1e6;
    }

    bool timed_out(uint16_t ms = 500) const {
        return !is_new(ms);
    }

protected:
    std::shared_ptr<rclcpp::Node> node_;
    rclcpp::Time time_point_;
};

template <typename T>
class CDataTimeStamp : public CTimeStamp {
public:
    CDataTimeStamp(const std::shared_ptr<rclcpp::Node>& node)
     : CTimeStamp(node), data_(T()) {}

    CDataTimeStamp(T data, const std::shared_ptr<rclcpp::Node>& node)
     : CTimeStamp(node), data_(data) {}

    void update(T data) {
        std::lock_guard<std::mutex> lock(data_mutex_);
        data_ = data;
        CTimeStamp::update();
    }

    T get() const {
        std::lock_guard<std::mutex> lock(data_mutex_);
        return data_;
    }

private:
    mutable std::mutex data_mutex_;
    T data_;
};

class CTimerTriggerOnce : public CTimeStamp {
public:
    explicit CTimerTriggerOnce(const std::shared_ptr<rclcpp::Node>& node)
     : CTimeStamp(node) {}

    bool start() {
        std::lock_guard<std::mutex> lock(trigger_mutex_);
        if (!running_) {
            running_ = true;
            triggered_ = false;
            update();
            DEBUG_STREAM_OUT("Timer trigger started");
            return true;
        }
        return false;
    }

    bool stop() {
        std::lock_guard<std::mutex> lock(trigger_mutex_);
        if (running_) {
            running_ = false;
            triggered_ = false;
            DEBUG_STREAM_OUT("Timer trigger stopped");
            return true;
        }
        return false;
    }

    bool check_trigger(uint16_t timeout_ms = 3000) {
        std::lock_guard<std::mutex> lock(trigger_mutex_);
        if (running_ && !triggered_) {
            if (timed_out(timeout_ms)) {
                triggered_ = true;
                DEBUG_OUT("Trigger activated after %ums", timeout_ms);
                return true;
            }
        }
        return false;
    }

    bool check_continuous(uint16_t interval_ms = 3000) {
        std::lock_guard<std::mutex> lock(trigger_mutex_);
        if (running_) {
            if (timed_out(interval_ms)) {
                update();  // Reset timer
                DEBUG_OUT("Continuous trigger at %ums interval", interval_ms);
                return true;
            }
        }
        return false;
    }

    void reset() {
        std::lock_guard<std::mutex> lock(trigger_mutex_);
        running_ = false;
        triggered_ = false;
        update();
        DEBUG_STREAM_OUT("Timer trigger reset");
    }

private:
    std::mutex trigger_mutex_;
    bool running_ = false;
    bool triggered_ = false;
};

} // namespace io_module

#endif // IO_MODULE__TIME_STAMP_HPP_